import cv2
import numpy as np
import math
# from pwm import CarMove


num_lane_point = 4

def initial_process(img):
    gray_img = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)  # rgb to gray
    img = cv2.blur(gray_img, (5, 5))  # 均值滤波
    retval, dst = cv2.threshold(img, 0, 255, cv2.THRESH_OTSU)  # 二值化
    # retval, dst = cv2.threshold(img, 50, 255, cv2.THRESH_BINARY)  # 二值化

    # dst = cv2.dilate(dst, None, iterations=3)  # 膨胀，白区域变大
    # dst = cv2.erode(dst, None, iterations=4)  # 腐蚀，白区域变小
    # cv2.line(dst, (0, 240), (640, 240), (0, 255, 0), 2)
    # cv2.line(dst, (0, 225), (640, 225), (0, 255, 0), 2)
    # cv2.line(dst, (0, 210), (640, 210), (0, 255, 0), 2)
    # cv2.line(dst, (0, 195), (640, 195), (0, 255, 0), 2)
    # cv2.imshow("img",dst)
    # cv2.waitKey(10)
    return dst

def decision_area(dst):

    height, width = dst.shape
    half_width = int(width / 2)
    right_line_pos = np.zeros((num_lane_point, 1))
    left_line_pos = np.zeros((num_lane_point, 1))

    for i in range(num_lane_point):  # 检测每一行上的像素点
        detect_height = int(height/3*2) - 12* (i + 1)
        detect_area_left = dst[detect_height,0: half_width - 1]  # 以宽度的中心将图像分为左右两个部分
        detect_area_right = dst[detect_height, half_width: width - 1]
        line_left = np.where(detect_area_left == 0)  # 检测像素为0的点的坐标
        line_right = np.where(detect_area_right == 0)
        # print(line_left,'and',line_right)
        if len(line_left[0]):
            left_line_pos[i] = int(np.max(line_left))  # 找出每一行左侧像素值为0的点的最大索引
        else:
            left_line_pos[i] = 0  # 如果没有检测到任何像素值为0的点，置左位置为0

        if len(line_right[0]):
            right_line_pos[i] = int(np.min(line_right))  # 找出每一行右侧像素值为0的点的最小索引
        else:
            right_line_pos[i] = half_width - 1  # 如果没有检测到任何像素值为0的点，置左位置为0

    left_max = np.max(left_line_pos)  # 以图片中心向两边搜索，找出左右两侧第一个像素为0的点
    right_min = np.min(right_line_pos)
    print(left_max,right_min)

    if left_max == 0 and right_min == half_width - 1:  # 如果左右黑线都检测不到
        angle=0
    elif left_max == 0:  # 如果检测不到左侧黑线
        if right_min > half_width - 100:  # 右侧黑线列坐标在540-640，认为仍是直行
            angle=0
        elif right_min < 100:  # 右侧黑线列坐标在320-420，以50度角左转
            angle=-80
        else:  # 右侧黑线列坐标在420-540，以30度角左转
            angle=-50
    elif right_min == half_width - 1:  # 如果检测不到右侧黑线
        if left_max < 100:  # 左侧黑线列坐标在0-100，认为仍是直行
            angle=0
        elif left_max > half_width - 100:  # 左侧黑线列坐标在220-320，以50度角右转
            angle=80
        else:  # 左侧黑线列坐标在220-320，以30度角右转
            angle=50
    else:  # 左右两侧都能检测到黑线，直行
        angle=0
    print(angle)
    return angle


def decision_k(dst):

    height, width = dst.shape
    half_width = int(width / 2)
    right_line_pos = np.zeros((num_lane_point, 1))
    left_line_pos = np.zeros((num_lane_point, 1))

    for i in range(num_lane_point):
        detect_height = int(height/2) - 15* (i + 1)
        detect_area_left = dst[detect_height,0: half_width - 1]
        detect_area_right = dst[detect_height, half_width: width - 1]
        line_left = np.where(detect_area_left == 0)
        line_right = np.where(detect_area_right == 0)
        # print(line_left,'and',line_right)
        if len(line_left[0]):
            left_line_pos[i] = int(np.max(line_left))
        else:
            left_line_pos[i] = 0

        if len(line_right[0]):
            right_line_pos[i] = int(np.min(line_right))
        else:
            right_line_pos[i] = half_width - 1
        # print(left_line_pos[i],right_line_pos[i])
    k_l0 =12/(left_line_pos[1]-left_line_pos[0])  # 计算左侧黑线的斜率
    k_r0 = 12 / (right_line_pos[1] - right_line_pos[0])  # 计算左侧黑线的斜率
    k_l1 = 12 / (left_line_pos[2] - left_line_pos[1])
    k_r1 = 12 / (right_line_pos[2] - right_line_pos[1])
    k_l= (k_l0+ k_l1)/2
    k_r = (k_r0 + k_r1) / 2
    # print(k_l,k_r)
    h_l = math.atan(k_l)  # 将斜率转换为弧度
    h_r= math.atan(k_r)
    a_l=int(math.degrees(h_l))  # 将弧度转换为角度
    a_r = int(math.degrees(h_r))
    if a_l > 0:  # 将由斜率所得角度转换为-90-90之间，方便计算最终控制角度
      a_l = 90-a_l
    else:
      a_l = -90-a_l
    if a_r > 0:
      a_r = 90 - a_r
    else:
      a_r = -90 - a_r
    # print(a_l, a_r)
    ############################ 决策 #####################################
    if a_l == 0 and a_r == 0:  # 如果左右黑线都检测不到
        angle=0
    elif a_l == 0:   # 如果检测不到左侧黑线
        if a_r>-15:  # 右侧角度在-15-0，直行
            angle=0
        else:   # 右侧角度在-90~-15，左转
            angle=a_r-25
            if angle < -90:
                angle = -90
    elif a_r == 0:  # 如果检测不到右侧黑线
        if a_l < 15:   # 左侧角度在0-15，直行
            angle=0
        else:  # 左侧角度在40-90，右转
            angle=a_l+25
            if angle > 90:
                angle = 90
    else:  # 左右两侧都能检测到黑线，直行
        angle=0
    print(angle)
    return angle


if __name__ == '__main__':
    img = cv2.imread('E:/python_work/imgs/img10.jpg')
    dst = initial_process(img)
    # decision_k(dst)
    theta= decision_k(dst)
    cv2.namedWindow('image', cv2.WINDOW_NORMAL)
    cv2.line(dst, (0,240), (640, 240), (0, 255, 0),2)
    cv2.line(dst, (0,225), (640, 225), (0, 255, 0),2)
    cv2.line(dst, (0,210), (640, 210), (0, 255, 0),2)
    cv2.line(dst, (0,195), (640, 195), (0, 255, 0),2)
    cv2.imshow('image', dst)
    cv2.waitKey(0)
    cv2.destroyAllWindows()
    print(theta)
# if no detected lane, then keep the last action
#  if only detected the right lane:
#  if the right lane is still close to the image border, then go straight;
#  if the right lane is too close to the image center, then spin around;
#  else, then turn and go straight
# if only detected the left lane: similar to the above
# if both lanes is detected: go straight
# if left_max == 0 and right_min == half_width - 1:
#     pass
# elif left_max == 0:
#     if right_min > half_width - 100:
#         ForB = 'Forward'
#         LorR = 'Brake'
#     elif right_min < 100:
#         ForB = 'Brake'
#         LorR = 'Left'
#     else:
#         ForB = 'Forward'
#         LorR = 'Left'
# elif right_min == half_width - 1:
#     if left_max < 100:
#         ForB = 'Forward'
#         LorR = 'Brake'
#     elif left_max > half_width - 100:
#         ForB = 'Brake'
#         LorR = 'Right'
#     else:
#         ForB = 'Forward'
#         LorR = 'Right'
# else:
#     ForB = 'Forward'
#     LorR = 'Brake'
#
# ############################ motion control #####################################
# if ForB is 'Brake':
#     if LorR is 'Left':
#         car.left(turn_left_speed)
#     elif LorR is 'Right':
#         car.right(turn_right_speed)
#     elif LorR is 'Brake':
#         car.brake()
# elif ForB is 'Forward':
#     if LorR is 'Left':
#         car.forward_turn(speed_low, speed_high)
#     elif LorR is 'Right':
#         car.forward_turn(speed_high, speed_low)
#     elif LorR is 'Brake':
#         car.forward(forward_speed)
# elif ForB is 'Backward':
#     if LorR is 'Left':
#         car.left(turn_left_speed)
#     elif LorR is 'Right':
#         car.right(turn_right_speed)
#     elif LorR is 'Brake':
#         car.back(40)
